#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Point
import A_star as A
import map_reading as mr
from prometheus_msgs.srv import PathPlan, PathPlanResponse

target_ = Point()
# 创建无人机相关数据变量
def path_plan_cb(req):
    target_.x = req.target.x
    target_.y = req.target.y
    target_.z = req.target.z
    # print(target_)
    occupancy_grid = mr.OccupancyGridReader()
    grid_target = occupancy_grid.real_to_grid(target_)
    planner = A.AStar(start = occupancy_grid.get_uav_grid_position(), target =  grid_target, map = occupancy_grid.get_map())
    path = planner.get_path(planner.heuristic_Manhattan)
    n = len(path)
    # 2024.1.23 间隔取点
    path_new = path[5:n:5]
    path_real = []
    for i in range(len(path_new)):
        temp_point = Point()
        temp_point.x = occupancy_grid.grid_to_real(path_new[i])[0]
        temp_point.y = occupancy_grid.grid_to_real(path_new[i])[1]
        temp_point.z = 2
        path_real.append(temp_point)
    # 目标点必须在路径中
    temp_point = Point()
    temp_point.x = occupancy_grid.grid_to_real(path[-1])[0]
    temp_point.y = occupancy_grid.grid_to_real(path[-1])[1]
    temp_point.z = 2
    path_real.append(temp_point)
    return PathPlanResponse(path_real)
    
if __name__ == "__main__":
    rospy.init_node('path_generator_server')
    s = rospy.Service('path_generator', PathPlan, path_plan_cb)
    rospy.loginfo("Ready to generate path.")
    rospy.spin()

